﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Simulador_Quadrotor
{
    public class ControlMotor
    {


        /// <summary>
        /// Controla las salidas al motor
        /// </summary>
        /// <param name="RotAnt"></param>
        /// <returns></returns>
        public double[] Controlar(double rotZ, double rotRoll, double rotPitch, double rotYaw, double errorZ)
        {
            double[] control = new double[4];
            double[] In = new double[]{
                rotZ,
                rotRoll,
                rotPitch,
                errorZ,
                rotYaw
            };
            if (In[0] < 0)
            {
                control[0] = 0;
                control[1] = 0;
                control[2] = 0;
                control[3] = 0;
            }
            else if (In[0] > 3)
            {
                control[0] = 3;
                control[1] = 3;
                control[2] = 3;
                control[3] = 3;
            }
            else if (In[3] <= 0.05)
            {
                control[0] = In[0] + In[2] + In[4];
                control[1] = In[0] - In[1] - In[4];
                control[2] = In[0] - In[2] + In[4];
                control[3] = In[0] + In[1] - In[4];
            }
            else
            {
                control[0] = In[0];
                control[1] = In[0];
                control[2] = In[0];
                control[3] = In[0];
            }
            return control;

        }

        public double Saturar(double entrada, double superior, double inferior)
        {
            if (entrada > superior)
            {
                return superior;
            }
            if (entrada < inferior)
            {
                return inferior;
            }
            return entrada;
        }
    }
}
